#include "stdafx.h"
#include "QuadTree.h"
#include "Collider.h"
#include "CollisionManager.h"

QuadTree::QuadTree(bool is_root, QuadTree* p_parent) : QT_NODE_CAPACITY(16), m_parent(p_parent), 
	m_north_east(nullptr), m_north_west(nullptr), m_south_east(nullptr), m_south_west(nullptr), m_collision_manager(nullptr) {
		if (is_root){
			m_root = this;
		}
		else {
			m_root = p_parent->GetRoot();
		}
}
QuadTree::~QuadTree(void){
	if (m_north_west){
		delete m_north_west;
		m_north_west = nullptr;
	}
	if (m_north_east){
		delete m_north_east;
		m_north_east = nullptr;
	}
	if (m_south_west){
		delete m_south_west;
		m_south_west = nullptr;
	}
	if (m_south_east){
		delete m_south_east;
		m_south_east = nullptr;
	}

	for (auto it = m_bounding_boxes.begin(); it != m_bounding_boxes.end(); it++)
	{
		delete *it;
		*it = nullptr;
	}
	m_bounding_boxes.clear();
}

XZ::XZ(float p_x, float p_z) : x(p_x), z(p_z) {}
XZBoundingBox::XZBoundingBox(const XZ& p_center, const XZ& p_half_dimension, void* p_user_data) : 
	center(p_center), half_dimension(p_half_dimension), user_data(p_user_data), owner(nullptr){}

void XZBoundingBox::Relocate(){
	QuadTree* parent = owner->GetRoot();
	if (parent)
		parent->Insert(this);
	else
		owner->Insert(this);
}

AABB::AABB(const XZ& p_center, const XZ& p_half_dimension) : center(p_center), half_dimension(p_half_dimension) {}
void AABB::Set(const XZ& p_center, const XZ& p_half_dimension){
	center = p_center;
	half_dimension = p_half_dimension;
}

bool AABB::Intersects(XZBoundingBox* bounding_box){
	float left_a = center.x - half_dimension.x;
	float right_a = center.x + half_dimension.x;
	float top_a = center.z + half_dimension.z;
	float bottom_a = center.z - half_dimension.z;

	float left_b = bounding_box->center.x - bounding_box->half_dimension.x;
	float right_b = bounding_box->center.x + bounding_box->half_dimension.x;
	float top_b = bounding_box->center.z + bounding_box->half_dimension.z;
	float bottom_b = bounding_box->center.z - bounding_box->half_dimension.z;

	if (bottom_a >= top_b) return false;
	if (top_a <= bottom_b) return false;
	if (right_a <= left_b) return false;
	if (left_a >= right_b) return false;
	return true;
}

bool AABB::Intersects(const AABB& aabb){
	float left_a = center.x - half_dimension.x;
	float right_a = center.x + half_dimension.x;
	float top_a = center.z + half_dimension.z;
	float bottom_a = center.z - half_dimension.z;

	float left_b = aabb.center.x - aabb.half_dimension.x;
	float right_b = aabb.center.x + aabb.half_dimension.x;
	float top_b = aabb.center.z + aabb.half_dimension.z;
	float bottom_b = aabb.center.z - aabb.half_dimension.z;

	if (bottom_a >= top_b) return false;
	if (top_a <= bottom_b) return false;
	if (right_a <= left_b) return false;
	if (left_a >= right_b) return false;
	return true;
}

void QuadTree::Set(const AABB& p_boundary){
	m_boundary = p_boundary;
}

bool QuadTree::Insert(XZBoundingBox* bounding_box){
	if (!m_boundary.Intersects(bounding_box))
		return false;
	
	if (m_north_west == nullptr){
		m_bounding_boxes.push_back(bounding_box);
		bounding_box->owner = this;
		return true;
	}
	int collision_counter = 0;
	if (m_north_west->Intersects(bounding_box)) collision_counter++;
	if (m_north_east->Intersects(bounding_box)) collision_counter++;
	if (m_south_west->Intersects(bounding_box)) collision_counter++;
	if (m_south_east->Intersects(bounding_box)) collision_counter++;
	if (collision_counter > 1){
		m_bounding_boxes.push_back(bounding_box);
		bounding_box->owner = this;
		return true;
	}

	if (m_north_west->Insert(bounding_box)) return true;
	if (m_north_east->Insert(bounding_box)) return true;
	if (m_south_west->Insert(bounding_box)) return true;
	if (m_south_east->Insert(bounding_box)) return true;

	if (m_parent){
		m_parent->Insert(bounding_box);
	}

	return false;
}

void QuadTree::QueryRange(std::vector<XZBoundingBox*>& list, AABB& range){

	if (!m_boundary.Intersects(range)){
		return;
	}
	for (unsigned int i = 0; i < m_bounding_boxes.size(); i++){
		if (range.Intersects(m_bounding_boxes[i])){
			list.push_back(m_bounding_boxes[i]);
		}
	}
	if (m_north_west == nullptr){
		return;
	}
	m_north_west->QueryRange(list, range);
	m_north_east->QueryRange(list, range);
	m_south_west->QueryRange(list, range);
	m_south_east->QueryRange(list, range);
}

void QuadTree::Subdivide(int subdivides){
	float half_dimension_x = m_boundary.half_dimension.x * 0.5f;
	float half_dimension_z = m_boundary.half_dimension.z * 0.5f;
	AABB north_east_aabb;
	north_east_aabb.center.x = m_boundary.center.x + half_dimension_x;
	north_east_aabb.center.z = m_boundary.center.z + half_dimension_z;
	north_east_aabb.half_dimension.x = half_dimension_x;
	north_east_aabb.half_dimension.z = half_dimension_z;
	AABB north_west_aabb;
	north_west_aabb.center.x = m_boundary.center.x - half_dimension_x;
	north_west_aabb.center.z = m_boundary.center.z + half_dimension_z;
	north_west_aabb.half_dimension.x = half_dimension_x;
	north_west_aabb.half_dimension.z = half_dimension_z;
	AABB south_east_aabb;
	south_east_aabb.center.x = m_boundary.center.x + half_dimension_x;
	south_east_aabb.center.z = m_boundary.center.z - half_dimension_z;
	south_east_aabb.half_dimension.x = half_dimension_x;
	south_east_aabb.half_dimension.z = half_dimension_z;
	AABB south_west_aabb;
	south_west_aabb.center.x = m_boundary.center.x - half_dimension_x;
	south_west_aabb.center.z = m_boundary.center.z - half_dimension_z;
	south_west_aabb.half_dimension.x = half_dimension_x;
	south_west_aabb.half_dimension.z = half_dimension_z;
	subdivides--;

	if (subdivides <= 0){
		return;
	}
	if (!m_north_east){
		m_north_east = new QuadTree(false, this);
		m_north_east->Set(north_east_aabb);
		m_north_east->Subdivide(subdivides);
	}
	if (!m_north_west){
		m_north_west = new QuadTree(false, this);
		m_north_west->Set(north_west_aabb);
		m_north_west->Subdivide(subdivides);
	}
	if (!m_south_east){
		m_south_east = new QuadTree(false, this);
		m_south_east->Set(south_east_aabb);
		m_south_east->Subdivide(subdivides);
	}
	if (!m_south_west){
		m_south_west = new QuadTree(false, this);
		m_south_west->Set(south_west_aabb);
		m_south_west->Subdivide(subdivides);
	}
}

void QuadTree::RemoveBoundingBox(XZBoundingBox* bounding_box){
	for (auto it = m_bounding_boxes.begin(); it != m_bounding_boxes.end(); it++){
		if ((*it) == bounding_box){
			delete bounding_box;
			m_bounding_boxes.erase(it);
			break;
		}
	}
}

void QuadTree::Update(){
	if (!m_bounding_boxes.empty()){
		for (unsigned int i = 0; i < m_bounding_boxes.size(); i++){
			if (!m_boundary.Intersects(m_bounding_boxes[i])){
				m_bounding_boxes[i]->Relocate();
				m_bounding_boxes.erase(m_bounding_boxes.begin() + i);
				i--;
			}
		}
	}

	if (m_north_west)
		m_north_west->Update();
	else
		return;
	m_north_east->Update();
	m_south_west->Update();
	m_south_east->Update();
}

void QuadTree::CheckCollision(){
	if (!m_bounding_boxes.empty()){
		std::vector<XZBoundingBox*> collision_list = m_bounding_boxes;
		if (m_parent){
			std::vector<XZBoundingBox*> parent_list;
			m_parent->GetBoundingBoxes(parent_list);
			if (!parent_list.empty()){
				collision_list.insert(collision_list.end(), parent_list.begin(), parent_list.end());
			}
		}
		for (unsigned int i = 0; i < collision_list.size(); i++){
			for (unsigned int j = 0; j < collision_list.size(); j++){
				if (collision_list[i] != collision_list[j]){
					Collider* collider = static_cast<Collider*>(collision_list[i]->user_data);
					Collider* collider2 = static_cast<Collider*>(collision_list[j]->user_data);
					if (collider->Intersects(collider2)){
						QuadTree* root = GetRoot();
						CollisionManager* collision_manager = root->GetCollisionManager();
						collision_manager->CheckCollision(collider->GetUserData(), collider2->GetUserData());
					}
				}
			}
		}
	}

	if (m_north_west)
		m_north_west->CheckCollision();
	else
		return;
	m_north_east->CheckCollision();
	m_south_west->CheckCollision();
	m_south_east->CheckCollision();
}

void QuadTree::GetBoundingBoxes(std::vector<XZBoundingBox*>& boxes){
	if (!m_bounding_boxes.empty()){
		boxes.insert(boxes.end(), m_bounding_boxes.begin(), m_bounding_boxes.end());
	}
	if (m_parent){
		m_parent->GetBoundingBoxes(boxes);
	}
}

bool QuadTree::Intersects(XZBoundingBox* bounding_box){
	return m_boundary.Intersects(bounding_box);
}